#include "zucmodule_multirobot_stat.h"
#include "zucmodule_stat.h"
#include "zucmodule_exceptions.h"
#include "nml_intf/zuc_nml.hh"
#include "zuc/nml_intf/zucpos2.h"

// 在name前面拼接Stat_前缀表示函数名称
#define ZUC_STAT_GET_FUNC_NAME(name) Stat_##name

#define ZUC_STAT_GETSET_DEF(name)                              \
    {                                                          \
        (char*)(#name), ((getter)ZUC_STAT_GET_FUNC_NAME(name)) \
    }
#define ZUC_STAT_GET_VAR(__name, __param, __varfun)                                                                                      \
    {                                                                                                                                    \
        (char*)(#__name), ((getter)[](PyObject * self, void* p)->PyObject * { return __varfun(((PyRobotStat*)self)->motion->__param); }) \
    }

#define ZUC_STAT_GET_LONG(__name, __param) ZUC_STAT_GET_VAR(__name, __param, PyLong_FromLong)
#define ZUC_STAT_GET_BOOL(__name, __param) ZUC_STAT_GET_VAR(__name, __param, PyBool_FromLong)

PyObject* Stat_act_jpos(PyRobotStat* s)
{
    double act_jpos[RobJointData<double>::MAX_AXIS] = {0};
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++) { act_jpos[i] = s->motion->joint[i].input; }
    return double_array(act_jpos, RobJointData<double>::MAX_AXIS);
}
PyObject* Stat_cmd_jpos(PyRobotStat* s)
{
    double act_jpos[RobJointData<double>::MAX_AXIS] = {0};
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++) { act_jpos[i] = s->motion->joint[i].output; }
    return double_array(act_jpos, RobJointData<double>::MAX_AXIS);
}
PyObject* Stat_act_pose(PyRobotStat* s) { return zucpose_array(s->motion->traj.actualPosition); }
PyObject* Stat_cmd_pose(PyRobotStat* s) { return zucpose_array(s->motion->traj.position); }
PyObject* Stat_model_data(PyRobotStat* s)
{
    PyObject* robstat = PyDict_New();
    auto jnum = s->motion->robot_joint_num;
    PyDict_SetItemString(robstat, "jointnum", PyLong_FromLong(jnum));
    PyDict_SetItemString(robstat, "serial", PyLong_FromLong(0));
    PyDict_SetItemString(robstat, "type", PyLong_FromLong(0));
    PyDict_SetItemString(robstat, "version", PyLong_FromLong(0));
    PyDict_SetItemString(robstat, "model", PyLong_FromLong(s->motion->robot_model_data.model));

    auto& model = s->motion->robot_model_data;
    PyDict_SetItemString(robstat, "payload", PyFloat_FromDouble(model.payload));
    PyDict_SetItemString(robstat, "max_traj_vel", PyFloat_FromDouble(model.maxTrajVel));
    PyDict_SetItemString(robstat, "max_traj_acc", PyFloat_FromDouble(model.maxTrajAcc));
    PyDict_SetItemString(robstat, "jpos_max", double_array(model.maxJointPosLimit, jnum));
    PyDict_SetItemString(robstat, "jpos_min", double_array(model.minJointPosLimit, jnum));
    PyDict_SetItemString(robstat, "jvel_max", double_array(model.maxJointVel, jnum));
    PyDict_SetItemString(robstat, "jacc_max", double_array(model.maxJointAcc, jnum));
    PyDict_SetItemString(robstat, "jerr_limit", double_array(model.joint_err_limit, jnum));

    return robstat;
}
PyObject* Stat_safe_pose_config(PyRobotStat* s)
{
    PyObject* sfpose = PyDict_New();
    auto jnum = RobJointVal::MAX_AXIS;
    PyDict_SetItemString(sfpose, "shrunken_pose", double_array(s->motion->shrunkenPose, jnum));
    PyDict_SetItemString(sfpose, "stretched_pose", double_array(s->motion->stretchedPose, jnum));
    PyDict_SetItemString(sfpose, "initial_pose", double_array(s->motion->initialPose, jnum));
    PyDict_SetItemString(sfpose, "initial_pose_tol", PyFloat_FromDouble(s->motion->initPoseTol));
    PyDict_SetItemString(sfpose, "vel_rate", PyFloat_FromDouble(s->motion->homeVel));

    return sfpose;
}
PyObject* Stat_safe_joint_config(PyRobotStat* s)
{
    PyObject* sf = PyTuple_New(RobJointData<double>::MAX_AXIS);
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        PyObject* jp = PyDict_New();
        PyDict_SetItemString(jp, "max_pos", PyFloat_FromDouble(s->motion->joint[i].maxPositionLimit));
        PyDict_SetItemString(jp, "min_pos", PyFloat_FromDouble(s->motion->joint[i].minPositionLimit));
        PyDict_SetItemString(jp, "max_vel", PyFloat_FromDouble(s->motion->joint[i].maxVel));
        PyDict_SetItemString(jp, "err_limit", PyFloat_FromDouble(s->motion->joint[i].err_limit));
        PyTuple_SetItem(sf, i, jp);
    }
    return sf;
}

PyObject* Stat_info(PyRobotStat* s)
{
    PyObject* robstat = PyDict_New();
    PyDict_SetItemString(robstat, "serialnum", PyLong_FromLong(s->motion->robot_serial_num));
    PyDict_SetItemString(robstat, "servo_version", PyLong_FromLong(s->motion->joint[0].servo_version));
    return robstat;
}

static int PyRobotStat_init(PyRobotStat* self, PyObject* args, PyObject* kwds) { return 0; }
static void PyRobotStat_dealloc(PyObject* self) { PyObject_Del(self); }
static PyMethodDef Stat_methods[] = {{NULL}};

// clang-format off
static PyGetSetDef Stat_getsetlist[] = {
    // 安全参数开始
    ZUC_STAT_GETSET_DEF(safe_joint_config),
    ZUC_STAT_GETSET_DEF(safe_pose_config),
    // 安全参数结束
    ZUC_STAT_GET_BOOL(enabled, is_enabled),
    ZUC_STAT_GET_BOOL(powered_on, powered_on),
    ZUC_STAT_GETSET_DEF(act_jpos),
    ZUC_STAT_GETSET_DEF(cmd_jpos),
    ZUC_STAT_GETSET_DEF(act_pose),
    ZUC_STAT_GETSET_DEF(cmd_pose),
    ZUC_STAT_GETSET_DEF(model_data),
    ZUC_STAT_GETSET_DEF(info),
    ZUC_STAT_GET_LONG(robot_joint_num, robot_joint_num),
    ZUC_STAT_GET_LONG(tool_id, currentToolId),
    ZUC_STAT_GET_LONG(payload_id, cur_payload_id),
    ZUC_STAT_GET_LONG(usrframe_id, currentUserId),
    ZUC_STAT_GET_LONG(baseframe_id, base_frame_id),
    ZUC_STAT_GET_BOOL(inpos, traj.inpos),
    {NULL}
    };
// clang-format on
PyTypeObject PyRobotStat_Type = {
    .ob_base = PyVarObject_HEAD_INIT(NULL, 0).tp_name = "jkzuc.robstat",
    .tp_basicsize = sizeof(PyRobotStat),
    .tp_dealloc = (destructor)PyRobotStat_dealloc,
    .tp_flags = Py_TPFLAGS_DEFAULT,
    .tp_methods = Stat_methods,
    .tp_getset = Stat_getsetlist,
    .tp_init = (initproc)PyRobotStat_init,
    .tp_new = PyType_GenericNew,
};
PyObject* RobotStat_CreateObject()
{
    PyObject* obj = (PyObject*)&PyRobotStat_Type;
    Py_XINCREF(obj);
    return obj;
}
